#ifndef CONTROL_H
#define CONTROL_H

int kp=5;

unsigned long McBSP0_slave(unsigned long Value);

void wheel_control(int left, int right)
{
	unsigned int code = 0x12DD;
	int left_dir=0;
	int right_dir=0;
	unsigned int tx_data=0;
			

	if(left > 0)
		left_dir = 1;
	else
		left_dir = 0;

	if(right > 0)
		right_dir = 1;
	else
		right_dir = 0;

	if(left < 0)
		left = -left;

	if(right < 0)
		right = -right;

    left=65+(int)(((double)35)/100*left);
	right=65+(int)(((double)35)/100*right);

	tx_data = (left << 9) || (left_dir << 8) || (right << 7) || left_dir;

	McBSP0_slave(code);
	McBSP0_slave(tx_data);

	


}
void control(int xpos,int ypos,int mode,int ref){
	int turn,left_wheel,right_wheel;
	if (mode==0){//tracking
		turn=-kp*(xpos-178);
		if (turn>500){
			turn=500;
		}
		if (turn<-500){
			turn=-500;
		}	
		if(turn>=0){
			left_wheel=ref;
			right_wheel=ref*(1000-turn);
		}
		else{
			left_wheel=ref*(1000+turn);
			right_wheel=ref;
		}	
	}
	if (mode==1){//left pivot
			left_wheel=0;
			right_wheel=ref;
	}
	if (mode==2){//right pivot
			left_wheel=ref;
			right_wheel=0;
	}
	left_wheel = left_wheel/10;
	right_wheel = right_wheel/10;
	wheel_control( left_wheel , right_wheel );
}

#endif 
